/*
 * SphericalCoordinateDifferenceFeature.cpp
 *
 *  Created on: Feb 23, 2011
 *      Author: johang
 */


#include "SphericalCoordinateDifferenceFeature.h"

SphericalCoordinateDifferenceFeature::SphericalCoordinateDifferenceFeature() {
	last_x = -1; last_y = -1;
	dims = 2;
	int sx = 61;
	int sy = 61;
	size_diff = sx * sy;
	diff_x = new int[size_diff];
	diff_y = new int[size_diff];
	int current_counter = 0;
//printf("---Setting up x y---\n");
	for(int cx = 1; cx <= sx/2;cx=(1.3f*cx)+1)
	{
		for(int cy = 1; cy <= sy/2;cy=(1.3f*cy)+1)
		{
			//printf("%i %i \n",cx,cy);
			diff_x[current_counter]=cx;
			diff_y[current_counter]=cy;
			current_counter++;

			diff_x[current_counter]=-cx;
			diff_y[current_counter]=-cy;
			current_counter++;
		}
	}
	size_diff = current_counter;
	current_counter = 0;
}

SphericalCoordinateDifferenceFeature::~SphericalCoordinateDifferenceFeature() {
	// TODO Auto-generated destructor stub
}

void SphericalCoordinateDifferenceFeature::getResponse(float * output,Point *** points, int x, int y,int * divi){
	last_x = x; last_y = y;
	//printf("%f %f\n",points[x][y]->norm_azimuth,points[x][y]->norm_zenith);
	float xdi = (points[x][y]->x - points[x+diff_x[current_counter]][y+diff_y[current_counter]]->x);
	float ydi = (points[x][y]->y - points[x+diff_x[current_counter]][y+diff_y[current_counter]]->y);
	float zdi = (points[x][y]->z - points[x+diff_x[current_counter]][y+diff_y[current_counter]]->z);

	output[0]=sqrt(xdi*xdi + ydi*ydi + zdi*zdi);//points[x][y]->norm_azimuth;

	xdi = (points[x][y]->norm_x * points[x+diff_x[current_counter]][y+diff_y[current_counter]]->norm_x);
	ydi = (points[x][y]->norm_y * points[x+diff_x[current_counter]][y+diff_y[current_counter]]->norm_y);
	zdi = (points[x][y]->norm_z * points[x+diff_x[current_counter]][y+diff_y[current_counter]]->norm_z);



	output[1]=1-fabs(xdi+ydi+zdi);
	/*
	if(output[1]<0.5)
	{
		printf("(%f,",points[x][y]->norm_x);
		printf("%f,",points[x][y]->norm_y);
		printf("%f) -",points[x][y]->norm_z);

		printf("(%f,",points[x+diff_x[current_counter]][y+diff_y[current_counter]]->norm_x);
		printf("%f,",points[x+diff_x[current_counter]][y+diff_y[current_counter]]->norm_y);
		printf("%f) -> ",points[x+diff_x[current_counter]][y+diff_y[current_counter]]->norm_z);

	}
	*/
	//printf("poss: %f angle: %f\n",output[0],output[1]);
	current_counter++;
}

extern bool is_valid(Point* p);

bool SphericalCoordinateDifferenceFeature::canGetResponse(Point *** points,int x, int y){
	if(current_counter < size_diff)
	{
		if	(
			 !((x+diff_x[current_counter])<0)
			&&!((y+diff_y[current_counter])<0)
			&&!((x+diff_x[current_counter])>=640)
			&&!((y+diff_y[current_counter])>=480)
			&&is_valid(points[x+diff_x[current_counter]][y+diff_y[current_counter]])
			)
		{
			return true;
		}
		else{	
			current_counter++;
			return canGetResponse(points,x,y);
		}
	}
	else{
		//printf("-------\n");
		current_counter=0;
		return false;
	}
};
